#include <rclcpp/rclcpp.hpp>
#include <turtlesim_msgs/srv/spawn.hpp>

using turtlesim_msgs::srv::Spawn;
using namespace std::chrono_literals;

// 生成乌龟
class Exer01_Spawn : public rclcpp::Node {
public:
  // 初始化
  Exer01_Spawn() : Node("Exer01_Spawn") {
    RCLCPP_INFO(this->get_logger(), "生成乌龟");

    // 声明并获取参数
    this->declare_parameter("x", 3.0);
    this->declare_parameter("y", 3.0);
    this->declare_parameter("theta", 0.0);
    this->declare_parameter("turtle_name", "turtle2");

    // 获取参数值
    x = this->get_parameter("x").as_double();
    y = this->get_parameter("y").as_double();
    theta = this->get_parameter("theta").as_double();
    turtle_name = this->get_parameter("turtle_name").as_string();

    // 创建客户端
    spawn_client_ = this->create_client<Spawn>("spawn");
  }

  // 检查是否连接上服务端
  bool check_client() {

    while (!spawn_client_->wait_for_service(2s)) {
      if (!rclcpp::ok()) {
        // 如果被中断，退出
        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "服务被中断，退出");
        return false;
      }
      // 等待服务
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "等待服务 spawn 启动...");
    }
    return true;
  }

  // 发送请求
  void send_request() {
    // 创建请求
    auto request = std::make_shared<Spawn::Request>();

    // 设置请求数据
    request->x = x;
    request->y = y;
    request->theta = theta;
    request->name = turtle_name; // 指定乌龟名称

    // 打印请求信息
    RCLCPP_INFO(this->get_logger(),
                "请求生成乌龟: x=%.2f, y=%.2f, theta=%.2f, name=%s", request->x,
                request->y, request->theta, request->name.c_str());
    // 发送请求
    auto result = spawn_client_->async_send_request(request);
    auto futurReturnCode = rclcpp::spin_until_future_complete(
        this->get_node_base_interface(), result);

    // 等待结果
    switch (futurReturnCode) {
    case rclcpp::FutureReturnCode::SUCCESS:

      // 获取结果
      if (result.valid()) {
        auto response = result.get();
        if (response->name != "") {
          RCLCPP_INFO(this->get_logger(), "乌龟生成成功 乌龟名称: %s",
                      response->name.c_str());
        } else {
          RCLCPP_ERROR(this->get_logger(), "乌龟名称重复，生成失败");
        }
      } else {
        RCLCPP_ERROR(this->get_logger(), "无效的future结果");
      }

      break;
    case rclcpp::FutureReturnCode::INTERRUPTED:
      RCLCPP_ERROR(this->get_logger(), "服务调用被中断");
      break;
    case rclcpp::FutureReturnCode::TIMEOUT:
      RCLCPP_ERROR(this->get_logger(), "服务调用超时");
      break;
    default:
      RCLCPP_ERROR(this->get_logger(), "服务调用失败，未知错误");
      break;
    }
  }

private:
  float x, y, theta;
  std::string turtle_name;
  rclcpp::Client<Spawn>::SharedPtr spawn_client_;
};

int main(int argc, char *argv[]) {
  // 初始化 ROS2
  rclcpp::init(argc, argv);
  // 创建节点
  auto node = std::make_shared<Exer01_Spawn>();
  RCLCPP_INFO(rclcpp::get_logger("Exer01_Spawn"), "客户端连接 服务端...");
  // 检查客户端是否连接上服务端
  if (!node->check_client()) {
    RCLCPP_INFO(rclcpp::get_logger("Exer01_Spawn"),
                "客户端连接 服务端 失败，退出");
    return -1;
  }
  // 发送请求
  node->send_request();

  // 保持节点运行
  rclcpp::spin(node);
  // 退出
  rclcpp::shutdown();
  return 0;
}
